#include  <msp430g2553.h>

#include "servo.h"
#include "gps.h"
//#include "i2c.h"
#include "display.h"

#define STATE_STEP1 0x00
#define STATE_STEP2 0x01
#define STATE_STEP3 0x02
#define STATE_STEP4 0x03
#define STATE_STEP5 0x04
#define STATE_STEP6 0x05
#define STATE_STEP7 0x06
#define STATE_FINAL 0x07
#define STATE_HORLOGE 0x08
#define STATE_LOOP 0x09

#define NB_STEP		7

#define I2C_DELAY 1

#define FLASH_BASE 0xE000

#define LOG_ADDR FLASH_BASE

float distance = 0;
int bearing = 0 ;

#define DEFAULT_LAT 39.034920
#define DEFAULT_LONG 125.754662

const float position_1 [] = {DEFAULT_LAT, DEFAULT_LONG};
const float position_2 [] = {DEFAULT_LAT, DEFAULT_LONG};
const float position_3 [] = {DEFAULT_LAT, DEFAULT_LONG};
const float position_4 [] = {DEFAULT_LAT, DEFAULT_LONG};
const float position_5 [] = {DEFAULT_LAT, DEFAULT_LONG};
const float position_6 [] = {DEFAULT_LAT, DEFAULT_LONG};
const float position_7 [] = {DEFAULT_LAT, DEFAULT_LONG};



const unsigned char display_init[] = { 0x00, 0x31, 0x14, 0x25, 0x56, 0x6D, 0x0C,
		0x06, 0x38, 0x01 };
const unsigned char line_1[] = { 0x00, 0x80 };
const unsigned char line_2[] = { 0x00, 0xC0 };
const unsigned char clear[] = { 0x00, 0x01 };

int byteCtr = 0;
unsigned char buffer_length = 0;
char * buffer_pointer;

char display_buffer[16];
const char * msg_hello = "HELLO";
const char * msg_etape = "STEP:\n  %d/%d";
const char * msg_att_gps = "WAIT\nGPS";
const char * msg_distance = "%d deg\n%fkm";
const char * msg_success = "TARGET\nREACHED";
const char * msg_final = "MISSION\nCOMPLETE";
const char * msg_an = "1 YEAR\nPASSED";
const char * msg_date = "%d/%d/%d\n   %d:%d";

float * target_position;

struct log {
	unsigned char state;
	unsigned char day;
	unsigned char month;
	unsigned char year;
}* pLog, *pNewLog;

int nb = 0;
unsigned int sSize = 0;

void display(unsigned int nb_bytes, char * buffer);
void i2c_send(unsigned char nb_bytes, char * buffer);
void state_machine(struct gps_fix fix);

void myDelay(unsigned int time_ms) {
	int c = 0;
	while (time_ms > 0) {
		for (c = 0; c < 3000; c++)
			;
		{
			_nop();
		}
		time_ms--;
	}
}

void setLedA() {
	P1OUT |= BIT0;
}

void clearLedA() {
	P1OUT &= ~BIT0;
}

void toggleLedA() {
	P1OUT ^= BIT0;
}

void saveState(struct log * newLog) {
	__bic_SR_register(GIE);
	pLog = (struct log *) LOG_ADDR;
	FCTL2 = FWKEY + FSSEL0 + FN5 + FN4; // MCLK/3 for Flash Timing Generator
	FCTL1 = FWKEY + ERASE; // Set Erase bit
	FCTL3 = FWKEY + LOCKA; // Clear LOCK & LOCKA bits
	pLog->state = 0x00; // Dummy write to erase Flash seg A
	FCTL1 = FWKEY + WRT; // Set WRT bit for write operation
	pLog->state = newLog->state;
	pLog->day = newLog->day;
	pLog->month = newLog->month;
	pLog->year = newLog->year;
	FCTL1 = FWKEY; // Clear WRT bit
	FCTL3 = FWKEY + LOCKA + LOCK; // Set LOCK & LOCKA bit
	__bis_SR_register(GIE);
}

// treat gps_fix provided by gps library
void new_fix(struct gps_fix fix) {
	setLedA();
	state_machine(fix);
	__bis_SR_register(CPUOFF + GIE);

}

void InitUART(void) {
	P1SEL |= (BIT1 + BIT2); // P1.1 = RXD, P1.2=TXD
	P1SEL2 |= (BIT1 + BIT2); // P1.1 = RXD, P1.2=TXD
	BCSCTL2 &= ~DIVS_3;
	UCA0CTL1 |= UCSSEL_2; // SMCLK

	// UCOS16 = 1
	// 8,000,000Hz, 9600Baud, UCBRx=52, UCBRSx=0, UCBRFx=1
	//UCA0BR0 = 8; // 16MHz, OSC16, 115200
	//UCA0BR1 = 0; // 16MHz, OSC16, 115200
	UCA0BR0 = 6; // 12MHz, OSC16, 115200
	UCA0BR1 = 0; // 12MHz, OSC16, 115200
	// UCA0MCTL = UCBRFx | UCBRSx | UCOS16
	UCA0MCTL = (11 << 4) | UCOS16; // UCBRFx=1,UCBRSx=0, UCOS16=1

	UCA0CTL1 &= ~UCSWRST; // **Initialize USCI state machine**
	IE2 |= UCA0RXIE; // Enable USCI_A0 RX interrupt
}

void TXdata(unsigned char c) {
	while (!(IFG2 & UCA0TXIFG))
		; // USCI_A0 TX buffer ready?
	UCA0TXBUF = c; // TX -> RXed character
}

void getDate(float date, unsigned char * day, unsigned char * month,
		unsigned char * year) {
	unsigned long dummy_date;
	dummy_date = (unsigned long) date;
	*day = dummy_date / 10000;
	*month = (dummy_date % 10000) / 100;
	*year = (dummy_date % 100);
}

void testDistance(struct gps_fix fix) {
	if (distance < 0.1) {
		myDelay(2000);
		if (pLog->state < STATE_FINAL) {
			i2c_send(2, (char *) clear);
			myDelay(I2C_DELAY);
			sSize = sprintf(display_buffer, msg_success);
			display(sSize, display_buffer);
			myDelay(I2C_DELAY);
			pNewLog->state = pLog->state + 1;
			getDate(fix.date, &(pNewLog->day), &(pNewLog->month),
					&(pNewLog->year));
			saveState(pNewLog);
			myDelay(2000);
			sSize = sprintf(display_buffer, msg_etape, pLog->state, NB_STEP);
			myDelay(I2C_DELAY);
			i2c_send(2, (char *) clear);
			myDelay(I2C_DELAY);
			display(sSize, display_buffer);
			myDelay(2000);
		}
	}
}

unsigned char testDate(float newDate) {
	unsigned char day, month, year;
	getDate(newDate, &day, &month, &year);
	return (year > pLog->year
			&& (month > pLog->month
					|| (month == pLog->month && day >= pLog->day)));
}

void showDistance() {
	distance_from(target_position[0], target_position[1], &distance, &bearing);
	sSize = sprintf(display_buffer, msg_distance, bearing, distance);
	i2c_send(2, ( char *) clear);
	myDelay(I2C_DELAY);
	display(sSize, display_buffer);
}

void showDate(float time, float date) {
	unsigned int day, month, year, hour, minute, dummy_time;
	unsigned long dummy_date;
	dummy_date = (unsigned long) date;
	day = dummy_date / 10000;
	month = (dummy_date % 10000) / 100;
	year = (dummy_date % 100);
	dummy_time = time / 100;
	hour = dummy_time / 100;
	minute = dummy_time % 100;
	sSize = sprintf(display_buffer, msg_date, day, month, year, hour, minute);
	i2c_send(2, (char *) clear);
	myDelay(I2C_DELAY);
	display(sSize, display_buffer);
}

void openLid() {
	enableServo();
	setPulse(POS_90);
	myDelay(4000);
	disableServo();
}

void state_machine(struct gps_fix fix) {
	switch (pLog->state) {
	case STATE_STEP1:
		target_position = (float *) position_1;
		showDistance();
		testDistance(fix);
		break;
	case STATE_STEP2:
		target_position = (float *) position_2;
		showDistance();
		testDistance(fix);
		break;
	case STATE_STEP3:
		target_position = (float *) position_3;
		showDistance();
		testDistance(fix);
		break;
	case STATE_STEP4:
		target_position = (float *) position_4;
		showDistance();
		testDistance(fix);
		break;
	case STATE_STEP5:
		target_position = (float *) position_5;
		showDistance();
		testDistance(fix);
		break;
	case STATE_STEP6:
		target_position = (float *) position_6;
		showDistance();
		testDistance(fix);
		break;
	case STATE_STEP7:
		target_position = (float *) position_7;
		showDistance();
		testDistance(fix);
		break;
	case STATE_FINAL:
		sSize = sprintf(display_buffer, msg_final);
		i2c_send(2, (unsigned char *) clear);
		myDelay(I2C_DELAY);
		display(sSize, display_buffer);
		openLid();
		pNewLog->state = pLog->state + 2;
		getDate(fix.date, &(pNewLog->day), &(pNewLog->month), &(pNewLog->year));
		saveState(pNewLog);
		break;
	case STATE_HORLOGE:
		showDate(fix.time, fix.date);
		if (testDate(fix.date)) {
			sSize = sprintf(display_buffer, msg_an);
			i2c_send(2, (unsigned char *) clear);
			myDelay(I2C_DELAY);
			display(sSize, display_buffer);
			openLid();
			pNewLog->state = pLog->state + 1;
			getDate(fix.date, &(pNewLog->day), &(pNewLog->month),
					&(pNewLog->year));
			saveState(pNewLog);
		}
		break;
	case STATE_LOOP:
		pNewLog->state = pLog->state - 1;
		saveState(pNewLog);
		break;
	default:
		break;
	}
}

unsigned char test_backdoor() {
	//test pin
	P2DIR &= ~BIT4;
	if ((P2IN & BIT4) == 0) {
		//printf("WARNING!");
		myDelay(4000);
		return ((P2IN & BIT4) == 0);
	}
	return 0;
}

void init_i2c() {
	P1SEL |= BIT6 + BIT7; // Assign I2C pins to USCI_B0
	P1SEL2 |= BIT6 + BIT7; // Assign I2C pins to USCI_B0
	UCB0CTL1 |= UCSWRST; // Enable SW reset
	UCB0CTL0 = UCMST + UCMODE_3 + UCSYNC; // I2C Master, synchronous mode
	UCB0CTL1 = UCSSEL_2 + UCSWRST; // Use SMCLK, keep SW reset
	//UCB0BR0 = 192; // fSCL = SMCLK/192 = ~100kHz
	UCB0BR0 = 144; // fSCL = SMCLK/144 = ~100kHz
	UCB0BR1 = 0;
	UCB0I2CSA = 0x3E; // Set slave address
	UCB0I2CIE = UCNACKIE | UCALIE;
	UCB0CTL1 &= ~UCSWRST; // Clear SW reset, resume operation
	IE2 |= UCB0TXIE | UCB0RXIE; // Enable TX ready interrupt
	byteCtr = 0;
}

void i2c_send(unsigned char nb_bytes, char * buffer) {
	byteCtr = 0;
	buffer_length = nb_bytes;
	buffer_pointer = buffer;
	UCB0CTL1 |= UCTR + UCTXSTT; // I2C TX, start condition
	__bis_SR_register(CPUOFF + GIE);
	while (byteCtr > 0)
		;
}

void display(unsigned int nb_bytes, char * buffer) {
	byteCtr = -1;
	if (nb_bytes > 8) {
		buffer_length = 8;
		buffer_pointer = buffer;
		UCB0CTL1 |= UCTR + UCTXSTT; // I2C TX, start condition
		__bis_SR_register(CPUOFF + GIE);
		myDelay(I2C_DELAY);
		i2c_send(2, (unsigned char *) line_2);
		myDelay(I2C_DELAY);
		display(nb_bytes - 8, &buffer[8]);
	} else {
		buffer_length = nb_bytes;
		buffer_pointer = buffer;
		UCB0CTL1 |= UCTR + UCTXSTT; // I2C TX, start condition
		__bis_SR_register(CPUOFF + GIE);
	}__bis_SR_register(CPUOFF + GIE);
	myDelay(I2C_DELAY);
	while (byteCtr > 0)
		;
}

void main(void) {

	WDTCTL = WDTPW + WDTHOLD; // Stop WDT
	BCSCTL1 = CALBC1_12MHZ; // Set DCO to 8Mhz
	DCOCTL = CALDCO_12MHZ; // Set DCO to 8Mhz
	P1DIR |= BIT0; //led
	clearLedA();
	InitUART();
	init_gps();
	pLog = (struct log *) LOG_ADDR;
	pNewLog = (struct log *) malloc(sizeof(struct log));
	if (pLog->state > STATE_LOOP) {
		pNewLog->state = STATE_STEP1;
		pNewLog->day = 0;
		pNewLog->month = 0;
		pNewLog->year = 0;
		saveState(pNewLog);
	}
	init_i2c();
	if (test_backdoor()) {
		__bis_SR_register(GIE);
		enableServo();
		setPulse(POS_90);
		myDelay(2500);
		disableServo();
		__bis_SR_register(GIE+CPUOFF);
		while (1)
			;
	}
	myDelay(1000); // delay for display startup
	i2c_send(11, (unsigned char *) display_init);
	myDelay(I2C_DELAY);
	i2c_send(2, (unsigned char *) clear);
	myDelay(I2C_DELAY);
	sSize = sprintf(display_buffer, msg_hello);
	display(sSize, display_buffer);
	enableServo();
	setPulse(POS_0);
	myDelay(2000);
	disableServo();
	sSize = sprintf(display_buffer, msg_etape, pLog->state, NB_STEP);
	i2c_send(2, (unsigned char *) clear);
	myDelay(I2C_DELAY);
	display(sSize, display_buffer);
	myDelay(2000);
	sSize = sprintf(display_buffer, msg_att_gps);
	i2c_send(2, (unsigned char *) clear);
	myDelay(I2C_DELAY);
	display(sSize, display_buffer);
	while (1) {
		parseStream(&gps_fifo);
		__bis_SR_register(CPUOFF + GIE);
	}
}

#pragma vector = USCIAB0TX_VECTOR
__interrupt void USCIAB0TX_ISR(void) {
	if (IFG2 & UCB0TXIFG) {
		if (byteCtr >= buffer_length) {
			UCB0CTL1 |= UCTXSTP; // I2C stop condition
			IFG2 &= ~UCB0TXIFG; // Clear USCI_B0 TX int flag
			byteCtr = 0;
		} else {
			if (byteCtr < 0) {
				UCB0TXBUF = 0x40; //access to data register of display
			} else {
				UCB0TXBUF = buffer_pointer[byteCtr];
			}
			byteCtr++;
		}
	}
	__bic_SR_register_on_exit(CPUOFF);
}

#pragma vector=USCIAB0RX_VECTOR
__interrupt void USCI0RX_ISR(void) {
	unsigned char ifg2 = IFG2;
	if (IFG2 & UCA0RXIFG) {
		unsigned char rec = UCA0RXBUF;
		fifo_write(&gps_fifo, rec);
		IFG2 &= ~UCA0RXIFG;
	}
	if (UCB0STAT & UCNACKIFG) {
		UCB0CTL1 |= UCTXSTP;
		UCB0STAT &= ~UCNACKIFG;
		byteCtr = -1;
	} else if (UCB0STAT & UCALIE) {
		UCB0CTL1 |= UCTXSTP;
		UCB0STAT &= ~UCALIE;
		byteCtr = -1;
	}
	__bic_SR_register_on_exit(CPUOFF);
}

